import numpy


def kinematics_state_function(self, t, p_x, p_y, p_z, q_0, q_1, q_2, q_3, v_x, v_y, v_z, omega_X, omega_Y, omega_Z, A_X, A_Y, A_Z, alpha_X, alpha_Y, alpha_Z, c_q):
    [g_x, g_y, g_z] = self.gravity(p_x,p_y,p_z).ravel()
    x0 = (1/2)*omega_X
    x1 = (1/2)*q_2
    x2 = (1/2)*q_3
    x3 = (1/2)*omega_Y
    x4 = (1/2)*omega_Z
    x5 = q_0**2
    x6 = q_1**2
    x7 = q_2**2
    x8 = q_3**2
    x9 = 1/(x5 + x6 + x7 + x8)
    x10 = 2*A_Y
    x11 = q_0*q_3
    x12 = q_1*q_2
    x13 = 2*A_Z
    x14 = q_0*x13
    x15 = q_3*x13
    x16 = 2*A_X
    return (numpy.array([v_x, v_y, v_z, c_q*q_0 - omega_Y*x1 - omega_Z*x2 - q_1*x0, c_q*q_1 - omega_Y*x2 + omega_Z*x1 + q_0*x0, c_q*q_2 + q_0*x3 - q_1*x4 + q_3*x0, c_q*q_3 + q_0*x4 + q_1*x3 - q_2*x0, x9*(A_X*x5 + A_X*x6 - A_X*x7 - A_X*x8 + g_x*x5 + g_x*x6 + g_x*x7 + g_x*x8 + q_1*x15 + q_2*x14 - x10*x11 + x10*x12), x9*(A_Y*x5 - A_Y*x6 + A_Y*x7 - A_Y*x8 + g_y*x5 + g_y*x6 + g_y*x7 + g_y*x8 - q_1*x14 + q_2*x15 + x11*x16 + x12*x16), x9*(A_Z*x5 - A_Z*x6 - A_Z*x7 + A_Z*x8 + g_z*x5 + g_z*x6 + g_z*x7 + g_z*x8 + q_0*q_1*x10 - q_0*q_2*x16 + q_1*q_3*x16 + q_2*q_3*x10), alpha_X, alpha_Y, alpha_Z]))

def ic_from_planetodetic(self, lamda_D, phi_D, h_D, V_N, V_E, V_D, psi, theta, phi, p_B, q_B, r_B):
    [p_x, p_y, p_z] = self.planetodetics.pd2pcf(lamda_D,phi_D,h_D).ravel()
    navigation_0 = (1 - self.planetodetics.f)**2
    navigation_1 = phi_D
    navigation_2 = -(1 - navigation_0)*numpy.sin(navigation_1)**2 + 1
    navigation_3 = h_D
    phidot_D = V_N/(self.planetodetics.a*navigation_0/navigation_2**(3/2) + navigation_3)
    lamdadot_D = V_E/((self.planetodetics.a/numpy.sqrt(navigation_2) + navigation_3)*numpy.cos(navigation_1))
    hdot_D = -V_D
    x0 = numpy.cos(phi)
    x1 = numpy.cos(lamda_D)
    x2 = numpy.cos(psi)
    x3 = x1*x2
    x4 = x0*x3
    x5 = numpy.sin(theta)
    x6 = numpy.cos(phi_D)
    x7 = x5*x6
    x8 = x1*x7
    x9 = numpy.sin(phi)
    x10 = numpy.sin(psi)
    x11 = x10*x6
    x12 = x11*x9
    x13 = numpy.sin(phi_D)
    x14 = numpy.cos(theta)
    x15 = x13*x14
    x16 = x0*x15
    x17 = numpy.sin(lamda_D)
    x18 = x10*x14
    x19 = x17*x18
    x20 = x10*x9
    x21 = x1*x5
    x22 = x20*x21
    x23 = x2*x7
    x24 = x0*x23
    x25 = x0*x17
    x26 = x10*x25
    x27 = x13*x26
    x28 = x15*x3
    x29 = x14*x6
    x30 = x29*x9
    x31 = x17*x30
    x32 = x13*x5
    x33 = x17*x2
    x34 = x33*x9
    x35 = x32*x34
    x36 = numpy.sqrt(x12 - x16 - x19 + x22 + x24 + x27 - x28 - x31 - x35 + x4 + x8 + 1)
    x37 = x3*x9
    x38 = x13*x20
    x39 = x0*x10
    x40 = x0*x33
    x41 = -x0*x11 - x15*x9 + x17*x38 - x21*x39 + x23*x9 + x25*x29 + x32*x40 + x37
    x42 = (1/2)/x36
    x43 = x2*x29
    x44 = x0*x1*x29
    x45 = x1*x38
    x46 = x26*x5
    x47 = x32*x4
    x48 = -x1*x13*x39 + x1*x18 + x1*x30 - x15*x33 + x17*x20*x5 + x17*x7 + x32*x37 + x40
    x49 = V_D*x6
    x50 = V_N*x13
    x51 = x32 - x34 + x43 + x44 + x45 + x46 + x47
    x52 = 1/(4*x12 - 4*x16 - 4*x19 + 4*x22 + 4*x24 + 4*x27 - 4*x28 - 4*x31 - 4*x35 + 4*x4 + 4*x8 + 4)
    x53 = x51**2*x52
    x54 = x41**2*x52
    x55 = x48**2*x52
    x56 = (1/4)*x12 - 1/4*x16 - 1/4*x19 + (1/4)*x22 + (1/4)*x24 + (1/4)*x27 - 1/4*x28 - 1/4*x31 - 1/4*x35 + (1/4)*x4 + (1/4)*x8 + 1/4
    x57 = 1/(x53 + x54 + x55 + x56)
    x58 = (1/2)*x51
    x59 = phidot_D*x56
    x60 = phidot_D*x1
    x61 = (1/2)*x48
    x62 = 1/(2*x12 - 2*x16 - 2*x19 + 2*x22 + 2*x24 + 2*x27 - 2*x28 - 2*x31 - 2*x35 + 2*x4 + 2*x8 + 2)
    x63 = x48*x62
    x64 = x41*x63
    x65 = phidot_D*x17
    x66 = x41*x51*x62
    x67 = (1/2)*x41
    x68 = x51*x63
    return (numpy.array([p_x, p_y, p_z, (1/2)*x36, x41*x42, x42*(-x32 + x34 - x43 - x44 - x45 - x46 - x47), x42*x48, -V_E*x17 - self.planetodetics.omega_p*p_y - x1*x49 - x1*x50, V_E*x1 + self.planetodetics.omega_p*p_x - x17*x49 - x17*x50, -V_D*x13 + V_N*x6, x57*(lamdadot_D*x58 + lamdadot_D*x64 + self.planetodetics.omega_p*x58 + self.planetodetics.omega_p*x64 + p_B*x53 + p_B*x54 + p_B*x55 + p_B*x56 + x17*x59 - x53*x65 + x54*x65 - x55*x65 - x60*x61 + x60*x66), x57*(lamdadot_D*x67 - lamdadot_D*x68 + self.planetodetics.omega_p*x67 - self.planetodetics.omega_p*x68 + q_B*x53 + q_B*x54 + q_B*x55 + q_B*x56 - x1*x59 - x53*x60 + x54*x60 + x55*x60 - x61*x65 - x65*x66), x57*(-lamdadot_D*x53 - lamdadot_D*x54 + lamdadot_D*x55 + lamdadot_D*x56 - self.planetodetics.omega_p*x53 - self.planetodetics.omega_p*x54 + self.planetodetics.omega_p*x55 + self.planetodetics.omega_p*x56 + r_B*x53 + r_B*x54 + r_B*x55 + r_B*x56 - x58*x65 + x60*x67 + x60*x68 + x64*x65)]))

def kinematics_output_function(self, t, p_x, p_y, p_z, q_0, q_1, q_2, q_3, v_x, v_y, v_z, omega_X, omega_Y, omega_Z):
    [lamda_D, phi_D, h_D] = self.planetodetics.pcf2pd(p_x,p_y,p_z).ravel()
    lamda_D = lamda_D - self.planetodetics.omega_p*t
    [W_N, W_E, W_D] = self.winds(t,lamda_D,phi_D,h_D).ravel()
    [rho, c_s, mu] = self.atmosphere(t,lamda_D,phi_D,h_D).ravel()
    V_NED_0 = self.planetodetics.omega_p*p_y + v_x
    V_NED_1 = phi_D
    V_NED_2 = numpy.sin(V_NED_1)
    V_NED_3 = self.planetodetics.omega_p*t
    V_NED_4 = numpy.sin(V_NED_3)
    V_NED_5 = lamda_D
    V_NED_6 = numpy.sin(V_NED_5)
    V_NED_7 = V_NED_4*V_NED_6
    V_NED_8 = numpy.cos(V_NED_3)
    V_NED_9 = numpy.cos(V_NED_5)
    V_NED_10 = V_NED_8*V_NED_9
    V_NED_11 = -self.planetodetics.omega_p*p_x + v_y
    V_NED_12 = V_NED_6*V_NED_8
    V_NED_13 = V_NED_4*V_NED_9
    V_NED_14 = numpy.cos(V_NED_1)
    V_NED_15 = v_z
    V_N = V_NED_0*(-V_NED_10*V_NED_2 + V_NED_2*V_NED_7) + V_NED_11*(-V_NED_12*V_NED_2 - V_NED_13*V_NED_2) + V_NED_14*V_NED_15
    V_E = V_NED_0*(-V_NED_12 - V_NED_13) + V_NED_11*(V_NED_10 - V_NED_7)
    V_D = V_NED_0*(-V_NED_10*V_NED_14 + V_NED_14*V_NED_7) + V_NED_11*(-V_NED_12*V_NED_14 - V_NED_13*V_NED_14) - V_NED_15*V_NED_2
    navigation_0 = (1 - self.planetodetics.f)**2
    navigation_1 = phi_D
    navigation_2 = -(1 - navigation_0)*numpy.sin(navigation_1)**2 + 1
    navigation_3 = h_D
    phidot_D = V_N/(self.planetodetics.a*navigation_0/navigation_2**(3/2) + navigation_3)
    lamdadot_D = V_E/((self.planetodetics.a/numpy.sqrt(navigation_2) + navigation_3)*numpy.cos(navigation_1))
    hdot_D = -V_D
    x0 = q_2**2
    x1 = q_3**2
    x2 = x0 + x1
    x3 = q_0**2
    x4 = q_1**2
    x5 = x3 + x4
    x6 = 1/(x2 + x5)
    x7 = -x3
    x8 = -x4
    x9 = x2 + x7 + x8
    x10 = lamda_D + self.planetodetics.omega_p*t
    x11 = numpy.sin(x10)
    x12 = numpy.cos(x10)
    x13 = q_0*q_3
    x14 = q_1*q_2
    x15 = 2*x13 + 2*x14
    x16 = numpy.cos(phi_D)
    x17 = q_1*q_3
    x18 = q_0*q_2
    x19 = 2*x17 - 2*x18
    x20 = numpy.sin(phi_D)
    x21 = x12*x9
    x22 = 2*x20
    x23 = x11*x16
    x24 = q_0*q_1
    x25 = q_2*q_3
    x26 = x24 + x25
    x27 = x4 + x7
    x28 = -x0
    x29 = x1 + x28
    x30 = x12*x16
    x31 = 2*x30
    x32 = -x1
    x33 = x0 + x32
    x34 = x17 + x18
    x35 = self.planetodetics.omega_p**2
    x36 = W_E*x12
    x37 = 2*v_y
    x38 = W_N*x16
    x39 = 2*v_z
    x40 = self.planetodetics.omega_p*p_x
    x41 = W_D*x20
    x42 = W_E*x11
    x43 = 2*v_x
    x44 = self.planetodetics.omega_p*p_y
    x45 = W_D*x30
    x46 = W_D*x23
    x47 = 2*x40
    x48 = 2*x44
    x49 = W_N*x22
    x50 = x12*x49
    x51 = x11*x49
    x52 = W_D**2 + W_E**2 + W_N**2 + p_x**2*x35 + p_y**2*x35 + v_x**2 + v_x*x50 + v_y**2 + v_y*x51 + v_z**2 - x36*x37 + x36*x47 - x37*x40 + x37*x46 - x38*x39 + x39*x41 - x40*x51 + x42*x43 + x42*x48 + x43*x44 + x43*x45 + x44*x50 + x45*x48 - x46*x47
    x53 = numpy.sqrt(x52*((x52>0.0)+0.5*(x52==0.0)))
    x54 = v_z - x38 + x41
    x55 = x3 + x8
    x56 = W_N*x20
    x57 = v_x + x12*x56 + x42 + x44 + x45
    x58 = 2*x57
    x59 = v_y + x11*x56 - x36 - x40 + x46
    x60 = x6*(2*x26*x54 + x58*(-x13 + x14) + x59*(x33 + x55))/x53
    x61 = phidot_D*x11
    x62 = 2*x17
    x63 = 2*x18
    x64 = phidot_D*x12
    x65 = 2*x64
    x66 = 2*lamdadot_D
    x67 = 2*self.planetodetics.omega_p
    x68 = 2*x61
    return (numpy.array([p_x, p_y, p_z, q_0, q_1, q_2, q_3, v_x, v_y, v_z, omega_X, omega_Y, omega_Z, lamda_D, phi_D, h_D, numpy.arctan2(x6*(x11*x9 + x12*x15), x6*(-x11*x15*x20 + x16*x19 + x20*x21)), -numpy.arcsin(x6*(-x15*x23 + x16*x21 + x22*(-x17 + x18))), numpy.arctan2(x6*(-x22*x26 + x23*(x27 + x29) + x31*(x13 - x14)), x6*(x20*(x27 + x33) + 2*x23*(x24 - x25) - x31*x34)), rho, c_s, mu, x53, numpy.arctan2(x6*(x34*x58 + x54*(x29 + x55) + 2*x59*(-x24 + x25)), x6*(x15*x59 + x19*x54 + x57*(x28 + x32 + x5))), numpy.arcsin(numpy.select([numpy.greater(x60, 1.0),numpy.less(x60, -1.0),numpy.greater(x53, 0.0)], [1.0,-1.0,x60], default=0.0)), x6*(-lamdadot_D*x62 + lamdadot_D*x63 + omega_X*x0 + omega_X*x1 + omega_X*x3 + omega_X*x4 - self.planetodetics.omega_p*x62 + self.planetodetics.omega_p*x63 + x0*x61 + x1*x61 + x13*x65 + x14*x65 - x3*x61 - x4*x61), x6*(omega_Y*x0 + omega_Y*x1 + omega_Y*x3 + omega_Y*x4 + x0*x64 - x1*x64 + x13*x68 - x14*x68 - x24*x66 - x24*x67 - x25*x66 - x25*x67 + x3*x64 - x4*x64), x6*(lamdadot_D*x0 - lamdadot_D*x1 - lamdadot_D*x3 + lamdadot_D*x4 + omega_Z*x0 + omega_Z*x1 + omega_Z*x3 + omega_Z*x4 + self.planetodetics.omega_p*x0 - self.planetodetics.omega_p*x1 - self.planetodetics.omega_p*x3 + self.planetodetics.omega_p*x4 - x24*x65 + x25*x65 - x61*x62 - x61*x63), V_N, V_E, V_D, W_N, W_E, W_D]))

def inertial_to_NED_dcm(self, t, lamda_D, phi_D):
    x0 = lamda_D + self.planetodetics.omega_p*t
    x1 = numpy.cos(x0)
    x2 = numpy.sin(phi_D)
    x3 = numpy.sin(x0)
    x4 = numpy.cos(phi_D)
    return (numpy.array([[-x1*x2, -x2*x3, x4], [-x3, x1, 0], [-x1*x4, -x3*x4, -x2]]))

def local_translational_trim_residual(self, p_x, p_y, p_z, q_0, q_1, q_2, q_3, v_x, v_y, v_z, A_X, A_Y, A_Z):
    [lamda_D, phi_D, h_D] = self.planetodetics.pcf2pd(p_x,p_y,p_z).ravel()
    [g_x, g_y, g_z] = self.gravity(p_x,p_y,p_z).ravel()
    V_NED_0 = phi_D
    V_NED_1 = numpy.cos(V_NED_0)
    V_NED_2 = v_z
    V_NED_3 = numpy.sin(V_NED_0)
    V_NED_4 = lamda_D
    V_NED_5 = numpy.cos(V_NED_4)
    V_NED_6 = self.planetodetics.omega_p*p_y + v_x
    V_NED_7 = V_NED_5*V_NED_6
    V_NED_8 = -self.planetodetics.omega_p*p_x + v_y
    V_NED_9 = numpy.sin(V_NED_4)
    V_NED_10 = V_NED_8*V_NED_9
    V_N = V_NED_1*V_NED_2 - V_NED_10*V_NED_3 - V_NED_3*V_NED_7
    V_E = V_NED_5*V_NED_8 - V_NED_6*V_NED_9
    V_D = -V_NED_1*V_NED_10 - V_NED_1*V_NED_7 - V_NED_2*V_NED_3
    navigation_0 = (1 - self.planetodetics.f)**2
    navigation_1 = phi_D
    navigation_2 = -(1 - navigation_0)*numpy.sin(navigation_1)**2 + 1
    navigation_3 = h_D
    phidot_D = V_N/(self.planetodetics.a*navigation_0/navigation_2**(3/2) + navigation_3)
    lamdadot_D = V_E/((self.planetodetics.a/numpy.sqrt(navigation_2) + navigation_3)*numpy.cos(navigation_1))
    hdot_D = -V_D
    x0 = q_0**2
    x1 = q_1**2
    x2 = q_2**2
    x3 = q_3**2
    x4 = 1/(x0 + x1 + x2 + x3)
    x5 = 2*x4
    x6 = q_0*x5
    x7 = q_2*x6
    x8 = q_1*x5
    x9 = q_3*x8
    x10 = -x7 + x9
    x11 = numpy.cos(phi_D)
    x12 = q_3*x6
    x13 = q_2*x8
    x14 = x12 + x13
    x15 = numpy.sin(lamda_D)
    x16 = numpy.sin(phi_D)
    x17 = x15*x16
    x18 = x1*x4
    x19 = x2*x4
    x20 = -x19
    x21 = x0*x4
    x22 = x3*x4
    x23 = x21 - x22
    x24 = x18 + x20 + x23
    x25 = numpy.cos(lamda_D)
    x26 = x16*x25
    x27 = q_1*x6
    x28 = q_2*q_3*x5
    x29 = x27 + x28
    x30 = -x12 + x13
    x31 = -x18
    x32 = x19 + x23 + x31
    x33 = x20 + x21 + x22 + x31
    x34 = -x27 + x28
    x35 = x7 + x9
    x36 = lamdadot_D*x16
    x37 = self.planetodetics.omega_p**2
    x38 = g_x + p_x*x37
    x39 = g_y + p_y*x37
    x40 = 2*self.planetodetics.omega_p
    x41 = x16*x40
    x42 = lamdadot_D*x11
    x43 = x11*x40
    x44 = x11*x15
    x45 = x11*x25
    return (numpy.array([A_X*(x10*x11 - x14*x17 - x24*x26) + A_Y*(x11*x29 - x17*x32 - x26*x30) + A_Z*(x11*x33 - x17*x34 - x26*x35) + V_D*phidot_D - V_E*x36 - V_E*x41 + g_z*x11 - x17*x39 - x26*x38, A_X*(x14*x25 - x15*x24) + A_Y*(-x15*x30 + x25*x32) + A_Z*(-x15*x35 + x25*x34) + V_D*x42 + V_D*x43 + V_N*x36 + V_N*x41 - x15*x38 + x25*x39, A_X*(-x10*x16 - x14*x44 - x24*x45) + A_Y*(-x16*x29 - x30*x45 - x32*x44) + A_Z*(-x16*x33 - x34*x44 - x35*x45) - V_E*x42 - V_E*x43 - V_N*phidot_D - g_z*x16 - x38*x45 - x39*x44]))

def inertial_to_body_dcm(q_0, q_1, q_2, q_3):
    x0 = q_2**2
    x1 = q_3**2
    x2 = q_0**2
    x3 = q_1**2
    x4 = x2 + x3
    x5 = 1/(x0 + x1 + x4)
    x6 = -x0
    x7 = -x1
    x8 = q_0*q_3
    x9 = q_1*q_2
    x10 = 2*x5
    x11 = q_1*q_3
    x12 = q_0*q_2
    x13 = x2 - x3
    x14 = q_0*q_1
    x15 = q_2*q_3
    return (numpy.array([[x5*(x4 + x6 + x7), x10*(x8 + x9), x10*(x11 - x12)], [x10*(-x8 + x9), x5*(x0 + x13 + x7), x10*(x14 + x15)], [x10*(x11 + x12), x10*(-x14 + x15), x5*(x1 + x13 + x6)]]))
